#pragma once


#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
#include "hnurm_interfaces/msg/vision_recv_data.hpp"
#include "hnurm_interfaces/msg/vision_send_data.hpp"
#include "hnurm_interfaces/msg/target.hpp"
#include "std_msgs/msg/float32.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "deque"
#include "mutex"
#include <atomic>
#include <vector>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
// #include "hnurm_decision_interfaces/msg/robot_status.hpp"

namespace hnurm_behavior_trees
{

/**
 * @brief The pubRobotStatus behavior is used to switch the controller
 * that will be used by the controller server. It subscribes to a topic "controller_selector"
 * to get the decision about what controller must be used. It is usually used before of
 * the FollowPath. The selected_controller output port is passed to controller_id
 * input port of the FollowPath
 */
class pubRobotStatus : public BT::SyncActionNode
{
public:
  /**
   * @brief A constructor for nav2_behavior_tree::pubRobotStatus
   *
   * @param xml_tag_name Name for the XML tag for this node
   * @param conf  BT node configuration
   */
  pubRobotStatus(
    const std::string & xml_tag_name,
    const BT::NodeConfiguration & conf);
  ~pubRobotStatus() override;
  /**
   * @brief Creates list of BT ports
   * @return BT::PortsList Containing basic ports along with node-specific ports
   */
  static BT::PortsList providedPorts()
  {
    return {
      BT::OutputPort<bool>("cruise_status","status to control entry of cruise branch"),
      BT::OutputPort<bool>("hp_status","status to control entry of add hp branch"),
      BT::OutputPort<bool>("pursuit_status","status to control entry of pursuit branch"),
      BT::OutputPort<bool>("pursuit_mode1","switch pursuit mode"),
      BT::OutputPort<bool>("pursuit_mode2","switch pursuit mode"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("target_goal","output target goal"),
      BT::OutputPort<bool>("is_hp_ok","check if hp is ok"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("cruise_goal","target cruise goal"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("supply_goal","supply goal"),
      BT::OutputPort<bool>("test","test")
    };
    
  }

  struct current_global_pose
  {
    float pose_x;
    float pose_y;
  };


  struct current_rpy {
      float roll;
      float pitch;
      float yaw;
  };

  struct current_referee_status
  {
    float current_hp;
    float game_progress;
    current_rpy rpy;
  };

  struct Send_data
  {
    bool target_state;
    float target_distance;
  };

  enum class PursuitType {
      Approach,   
      Away    
  };
  std::atomic<PursuitType> pursuit_type = PursuitType::Approach;
  // 控制权优先级枚举
  enum class ControlAuthority {
      Idle,         // 空闲状态
      MainCamera,   // 主相机（最高优先级）
      BackCamera    // 后视相机（低优先级）
  };

  struct current_cruise_goal
  {
    int idx;
    bool former_is_reached;
  };
  
  struct referen_yaw_control
  {
    double reference_yaw_;
    bool on_control;
  };

  // 全局控制权状态
  std::atomic<ControlAuthority> current_authority = ControlAuthority::Idle;
  // 全局变量存储基准Yaw角度
  referen_yaw_control back_camera_control = {0.0,false};
  std::atomic<bool> on_control = false;
   
  Send_data send_data{false,0.0};
  
  current_global_pose current_pose_{ 0.0,0.0};

  current_referee_status referee_status_{0.0,0.0,{0.0f,0.0f,0.0f}
  };
  float back_target_bias = 0.0;


  //FSM global status
  std::atomic<bool> pursuit_status = false;
  std::atomic<bool> cruise_status = false;
  std::atomic<bool> hp_status = false;

  bool pursuit_ = false;
  bool cruise_ =false;
  bool supply_ =false;
  bool stay_ = false;

  //cruise goal iterator
  current_cruise_goal cruise_iterator{0,false};
  bool is_waiting_ = false;
  double wait_duration_ = 2.0;
  rclcpp::Time wait_start_time_;
  double timeout_ = 10.0;
  rclcpp::Time timeout_start_time_;

  //tfs
  geometry_msgs::msg::TransformStamped transform_m2bf;  //transform map to base_footprint



  geometry_msgs::msg::PoseStamped supply_goal;

private:
  /**
   * @brief Function to perform some user-defined operation on tick
   */
  BT::NodeStatus tick() override;

  /**
   * @brief callback function for the controller_selector topic
   *
   * @param msg the message with the id of the controller_selector
   */
  void recv_callback(const hnurm_interfaces::msg::VisionRecvData::SharedPtr msg);
  void back_target_callback(const std_msgs::msg::Float32::SharedPtr msg);
  void send_callback(const hnurm_interfaces::msg::VisionSendData::SharedPtr msg);
  void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
  void target_callback(const hnurm_interfaces::msg::Target::SharedPtr msg);

  void global_pose_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
  void timer_callback();
  void tf_listener_callback();

  void all_off();
  bool goal_checker(current_global_pose cruise_goal,current_global_pose current_pose);
  geometry_msgs::msg::PoseStamped update_goal(current_global_pose cruise_goal); 
  current_global_pose find_Away_point(float yaw,float distance);
  //subscribers
  rclcpp::Subscription<hnurm_interfaces::msg::VisionRecvData>::SharedPtr recv_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr back_target_sub_;
  rclcpp::Subscription<hnurm_interfaces::msg::VisionSendData>::SharedPtr send_sub_;

  rclcpp::Subscription<hnurm_interfaces::msg::Target>::SharedPtr target_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;

  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr global_pose_sub_;
 
 //publihser
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pursuit_pub_;
  rclcpp::Publisher<hnurm_interfaces::msg::VisionSendData>::SharedPtr decision_pub_;



  rclcpp::Node::SharedPtr node_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::TimerBase::SharedPtr tf_listener_timer_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  // declare subscriptions
  std::string recv_topic_;
  std::string back_target_;
  std::string send_topic_;
  std::string target_topic_;
  std::string global_position_;
  std::string pursuit_armor_id_;
  std::string decision_send_topic_;
  std::string odom_topic_;
  float hp_threshold;
  std::vector<current_global_pose> cruise_goals_;

  std::mutex transform_mutex;
  std::thread executor_thread_;

  std::mutex mtx_;
  std::thread wait_thread_;

  std::deque<float> bias_window_;  // slide window
  const int WINDOW_SIZE = 10;       
};
}  // namespace hnurm_behavior_trees

